COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	put locations into comtab
C00003 00003	 ROUTINE TO TAKE A MOVE COMMAND GIVEN IN TERMS OF A SERIES OF TRANS ADDRESSES
C00014 00004		RCENTER
C00015 00005	 ROUTINE TO TAKE A DRIVE COMMAND GIVEN IN A RELATIVE OR ABSOLUTE AMOUNT
C00022 00006	FOLLOWING CONTAIN JOINT ANGLES FOR BLUE AND YELLOW ARMS AT THE NEW TRANSES
C00024 00007	ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
C00026 ENDMK
C⊗;
;put locations into comtab

DATA
PUTLOC	LRPMOVE,RPMOVE
PUTLOC	LRTADRIVE,RTADRIVE
PUTLOC	LRTDDRIVE,RTDDRIVE
PUTLOC	LRCENTER,RCENTER
PUTLOC	LRFORCE, RFORCE
CODE
; ROUTINE TO TAKE A MOVE COMMAND GIVEN IN TERMS OF A SERIES OF TRANS ADDRESSES
;AND CONVERTING IT INTO A POLYNOMIAL COEFFICIENT FORM

;DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
;
;THE FOLLOWING DESCRIBES THE REQUIRED ORGANIZATION FOR THE TRANS 
;DATA LIST AND ASSOCIATED SERVO POINTERS:
;
;	SERVO POINTERS		DATA ARRAY
;
;				XXXXXX		TWO SERVO BIT WORDS
;				XXXXXX		  1 BIT FOR EACH SERVO
;				BITS		COMMAND MODE BITS, EG. NNUL,WOBBLE
;				WOBMAG		PTR TO WOBBLE MAGNITUDE CELL
;				TRANS		POINTER TO LIST OF END POINT
;						  TRANSFORMS AND VALIDITY NUMBERS
;				CODE		PTR TO CODE TO BE SCHEDULED THIS SEG
;				TIME		LENGTH OF SEGMENT IN MSECS
;						OR -SPEEDFACTOR*1000
;						0 MEANS SPEEDFACTOR=1 AND NO SPECIFIED TIME
;				TRANS		POINTER TO LIST OF END POINT
;						  TRANSFORMS AND VALIDITY NUMBERS
;				CODE		PTR TO CODE TO BE SCHEDULED THIS SEG
;				TIME		LENGTH OF SEGMENT IN MILLISEC
;				.
;				.
;				.
;				.
;				0
;						END OF MOVE SEGMENTS
;
;
;
;	THE CALLING SEQUENCE FOR THIS ROUTINE IS THE SAME AS IN MOVE
;
;		MOV	#COFLST,R0	;SET POINTER TO DATA LIST
;		MOV	#DEVICE,R1	;STORAGE AREA FOR DEVICE BLOCK,33 WORDS
;					; WHICH WILL BE SAME AS THE ONE PASSED TO MOVE
;		JSR	PC,PMOVE
;		TST	R0		;CHECK FOR ERROR CONDITION
;
;
;	RPMOVE is used when position independent code is desired.
;	(R4) contains the value of the interpreter program counter, and
;	R0 the amount of offset in words of the coeflist	[MSM 1/24/79]
;
;REGISTERS USED
;		R0,R1 PASS ARGUMENTS AND THE STATE IS AS RETURNED FROM MOVE
;		AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
;
;

CODE
;RELATIVE DISTANCE ACROSS SEGMENTS IN POLY COEF LIST
SGTTME	==4	;OFFSET OF TIME IN MILLISEC FOR THIS SEGMENT
SGTCDE	==2	;OFFSET FOR CODE
SGTTRN	==0	;OFFSET FOR TRANS
SGTNXT	==6	;OFFSET TO NEXT SEGMENT
SEGOFF	==310 	;SPAN OF COEFFICIENTS
A4COEF	==30	;A4 TERM OF FIRST POLYNOMIAL

RPMOVE:			; now has absolute values
;	ASL	R0	; get byte offset
;	ADD	(R4),R0	; get the right address
				; actually @IPC(R4), but IPC=0
PMOVE:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	R1,-(SP)

;COPY OUT HEADER INFO FROM THE TRANS LIST

	MOV	R0,R5		;USE R5 AS POINTER TO TRANS LIST
	MOV	#PLYBUF,R4	;USE R4 AS PTR TO POLY LIST FOR MOVE

	MOV	#4,R3		;COPY FIRST 4 PIECES OF HEADER INFO
1$:	MOV	(R5)+,(R4)+
	SOB	R3,1$
				;AT THIS POINT R5 POINTS TO THE TRANSFORM
				;R4 POINTS TO  DATPT{PTR}
NXTTR0:	MOV	#7,R3		;MOVE CURRENT VALUES OF JOINT ANGLES INTO
	MOV	#BTH,R2		;BJTHF AND YJTHF
	MOV	#BJTHF,R1
1$:	LDF	@(R2)+,AC0
	STF	AC0,(R1)+
	SOB	R3,1$
NXTTRN:	JSR	PC,CSOLVE	;DO A SOLVE
	TST	R0		;TEST AND CHECK IF SOLVE DID THE RIGHT THING
	BEQ	11$
	ALERR	NOSOLM		;complain 
	TST	R0
	BEQ	11$		;cleared by user, continue
	MOV	#NOSOLU,R0	;ELSE SIGNAL NO ARM SOLUTION FOUND
	MOV	(SP)+,R1	;clear out stack
	JMP	PMDNE		; and exit cleanly
11$:	LDF	FM10,AC3	;AC3←-10
	LDF	FM1P5,AC2
	STF	AC2,AC4		;AC4←-15/10
	LDF	FM0P4,AC2
	STF	AC2,AC5		;AC5←6/(-15)
	CLRF	AC2		;USED TO STORE MAX TIME SO FAR

	MOV	#BTIMFAC,R1	;USED TO GET TIME FACTORS
	MOV	#BJTHF,R2	;R2 POINTS TO VALUES AT END OF SEGMENT
	MOV	R4,R0		;R0 IS A POINTER THAT OUTPUTS INDIVIDUAL COEF
	ADD	#A0COEF,R0	;SET UP THE VALUE TO OUTPUT
	MOV	#6,R3		;COMPUTE CHANGE IN ANGLES AND OUTPUT THE RELEVANT VALUES
2$:	LDF	BJTHO-BJTHF(R2),AC0
	STF	AC0,(R0)+	;OUTPUT A0
	CLRF	(R0)+		;OUTPUT A1
	CLRF	(R0)+		;OUTPUT A2
	SUBF	(R2)+,AC0	;AC0←BJTHO-BJTHF
	LDF	(R1)+,AC1	;AC1 NOW HAS TIMFAC
	MULF	AC0,AC1		;AC1 NOW HAS TIME TAKEN FOR THIS JOINT
	ABSF	AC1		;TAKE THE ABSOLUTE VALUE
	CMPF	AC1,AC2		;COMPARE THIS VALUE TO VALUE SO FAR
	CFCC			;COPY FLOATING CONDITION CODES
	BLT	5$		;TIME FOR THIS JOINT SMALLER?
	LDF	AC1,AC2		;NO, UPDATE AC2
5$:	MULF	AC3,AC0		;AC0←A3= (BJTHF-BJTHO)*10
	STF	AC0,(R0)+	;OUTPUT A3
	MULF	AC4,AC0		;AC0←A4
	STF	AC0,(R0)+	;OUTPUT A4
	MULF	AC5,AC0		;AC0←A5
	STF	AC0,(R0)+	;OUTPUT A5
	SOB	R3,2$

	MULF	SPEED,AC2	;MULTIPLY BY DEFAULT SPEED FACTOR
	STCFI	AC2,R1		;CONVERT TIME INTO INTEGER MILLISECONDS
	MOV	SGTTME(R5),R2	;LOOK AT TIME SPECIFIED FOR MOTION
	BLT	6$		;IS IT TIME OR SPEED FACTOR
	CMP	R1,R2		;IS OUR TIME GREATER THAN USER GIVEN TIME?
	BLT	7$
	MOV	R1,R2		;YES, USED OUT TIME
7$:	BR	8$		;NO, USE USERS TIME
6$:	MUL	R1,R2
	DIV	#-1000.,R2	;SPEED FACTOR WAS MULTIPLIED BY -1000
8$:	ADD	#400.,R2	;ADD .4 SEC SLACK TIME
	CMP	#600.,R2	;COMPARE WITH .6 SEC TIME
	BLT	9$
	MOV	#600.,R2	;MIN TIME FOR TRAJ=.6 SEC
9$:	MOV	R2,SEGTME(R4)	;PUSH INTO THE POLYNOMIAL LIST
	MOV	#SEGOFF,(R4)	;SET UP POINTER TO NEXT SEGMENT POINTER
;	MOV	SGTTRN(R5),SEGTRN(R4);TRANSFER TRANS NUMBER
	CLR	SEGTRN(R4)	;FOR POINTY THIS IS ZERO
;	MOV	SGTCDE(R5),RNCODE(R4)	;RUNCODE OFFSET
	CLR	RNCODE(R4)	;FOR POINTY THIS IS ZERO

; GET READY TO CALL BEJCZY
	MOV	#6,R3		;SETUP UP DARRAY PRIOR TO CALLING BEJCZY
	MOV	#DARRAY,R1	;DARRAY IS TO BE SET UP WITH ADDRESSES OF
3$:	MOV	R0,(R1)+	; FOLLOWING
	ADD	#4,R0		;D1,D2,D3,...D6,D11,D22,D33,D44,D55,D66
	MOV	R0,12(R1)
	ADD	#4,R0
	SOB	R3,3$

	MOV     #BTHF,R0	;PTR TO LIST OF PTRS TO JOINT ANGLES
	MOV	#DARRAY,R1	;PTR TO LIST OF PTRS TO D(I)'S AND D(II)'S
	MOV	#BLUARM,R2    	;MECHANISM BIT INDICATING DEVICE TO USE
	JSR	PC,DTERMS		;CALL BEJCZY ROUTINES

;	CLRF	@(R1)		; THESE THREE STATEMENTS ARE KLUDGES BECAUSE
;				; BEJCZY ROUTINE ASSUMES THAT DI(1) AND DI(6)
;	CLRF	@12(R1)		; ARE 0 AND DOESNT BOTHER TO CHANGE THEM

	ADD	#SEGOFF,R4	;OFFSET R4 BY RIGHT AMOUNT
	ADD	#SGTNXT,R5	;JUMP R5 TO THE NEXT SEGMENT TRANS INFO
	CMP	(R5),#0
	BEQ	4$		;ANY MORE TRANSES TO COMPUTE?
	JMP	NXTTRN		;YUP, NOT OVER YET

4$:	CLR	(R4)		;NOPE,OUTPUT FINAL ZERO TO INDICATE NO MORE SEGMENTS
	MOV	#PLYBUF,R0	;SETUP TO CALL MOVE
	MOV	(SP)+,R1	;GET THE DEVICE BLOCK ADRESS
	JSR	PC,MOVE		;WAVE THE ARM
PMDNE:	MOV	(SP)+,R5	;RESTORE REGISTERS
	MOV	(SP)+,R4	;R0,R1 WILL BE IN THE STATE IN WHICH THEY LEFT MOVE
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC		;EXIT


;ROUTINE TO SAVE JOINT ANGLES AT BEGINNING OF THIS SEGMENT (BJTHF) INTO BJTHO AND CALL SOLVE
;R5 CONTAINS THE ADDRESS OF THE REQUIRED TRANS

CSOLVE:	MOV	#7,R3		;FIRST COPY VALUES AT BEG OF SEG INTO BJTHO
	MOV	#BJTHO,R0
	MOV	#BJTHF,R1
1$:	LDF	(R1)+,AC0
	STF	AC0,(R0)+
	SOB	R3,1$
	MOV	(R5),R0		;SETUP CALL TO SOLVE
	MOV	R4,-(SP)	;setup right value for environment
	MOV	10(SP),R4
	JSR	PC,@LGETARG	;find the address of the transform
	MOV	(SP)+,R4
	MOV	2(R0),R0	;R0←LOC[trans]
	MOV	#THETFP,R1
	MOV	#BLUARM,R2	;MECHANISM
	JSR	PC,SOLVE
;	TST	R0		;CHECK WHETHER CAN MAKE IT TO THIS TRANSFORM
	RTS	PC
;	RCENTER

RCENTER:		; now has absolute values
;	ASL	R0
;	ADD	(R4),R0	; get absolute address
	JMP	CENTER

; ROUTINE TO TAKE A DRIVE COMMAND GIVEN IN A RELATIVE OR ABSOLUTE AMOUNT
;AND CONVERTING IT INTO A POLYNOMIAL COEFFICIENT FORM

;DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
;
;THE FOLLOWING DESCRIBES THE REQUIRED ORGANIZATION FOR THE TRANS 
;DATA LIST AND ASSOCIATED SERVO POINTERS:
;
;	SERVO POINTERS		DATA ARRAY
;
;				XXXXXX		TWO SERVO BIT WORDS
;				XXXXXX		  1 BIT FOR EACH SERVO
;				BITS		COMMAND MODE BITS, EG. NNUL,WOBBLE
;				WOBMAG		PTR TO WOBBLE MAGNITUDE CELL
;			        SAMOUNT		POINTER TO A SCALAR
;				CODE		PTR TO CODE TO BE SCHEDULED THIS SEG
;				TIME		LENGTH OF SEGMENT IN MSECS
;						OR -SPEEDFACTOR*1000
;						0 MEANS SPEEDFACTOR=1 AND NO SPECIFIED TIME
;				0
;						END OF MOVE SEGMENTS
;
;	THE CALLING SEQUENCE FOR THIS ROUTINE IS THE SAME AS IN MOVE
;
;		MOV	#COFLST,R0	;SET POINTER TO DATA LIST
;		MOV	#DEVICE,R1	;STORAGE AREA FOR DEVICE BLOCK,33 WORDS
;					; WHICH WILL BE SAME AS THE ONE PASSED TO MOVE
;		JSR	PC,TMOVE
;		TST	R0		;CHECK FOR ERROR CONDITION
;
;
;REGISTERS USED
;		R0,R1 PASS ARGUMENTS AND THE STATE IS AS RETURNED FROM MOVE
;		AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
;
;


;RELATIVE DISTANCE ACROSS SEGMENTS IN POLY COEF LIST
SGJTME	==4	;OFFSET OF TIME IN MILLISEC FOR THIS SEGMENT
SGJCDE	==2	;OFFSET FOR CODE
SGJTRN	==0	;OFFSET FOR TRANS
SGJOFF	==40. 	;SPAN OF COEFFICIENTS
SGJDAT  ==8.	;BEGINNING OF DATA FROM COEF LIST


RTADRIVE:
;	ASL	R0
;	ADD	(R4),R0
TADRIVE: MOV	#1,ABSDRV	;SET UP FIRST PART OF HEADER INFO
	BR	PDRIVE

RTDDRIVE:
;	ASL	R0
;	ADD	(R4),R0
TDDRIVE: MOV	#0,ABSDRV

PDRIVE:	MOV	R2,-(SP)	;SAVE REGISTERS
	MOV	R3,-(SP)
	MOV	R4,-(SP)
	MOV	R5,-(SP)
	MOV	R1,-(SP)

	MOV	R0,R5		;USE R5 AS POINTER TO DATA LIST
	MOV	#PLYBUF,R4	;USE R4 AS PTR TO POLY LIST FOR MOVE

	MOV	#4,R3		;COPY FIRST 4 PIECES OF HEADER INFO
1$:	MOV	(R5)+,(R4)+
	SOB	R3,1$
	MOV	R4,-(SP)	;SAVE POINTER FOR FUTURE USE

	MOV	(R0),R2		;GET THE SERVO BITS TO DETERMINE # OF REQ. SRV.
       	MOV	2(R0),R3	;32 POSSIBLE SERVOS
	MOV	#-1,R4
11$:	INC	R4		;INCREMENT SERVO COUNT
	ASHC	#1,R2		;GET  NEXT SERVO BIT
	BNE	11$		;REPEAT TILL ALL SERVO BITS ADDED
	ADD	R4,R4		;DOUBLE TO GET THE OFFSET

	MOV	#7,R3		;MOVE CURRENT VALUES OF JOINT ANGLES INTO
	MOV	#BTH,R2		;BJTHF AND YJTHF
	MOV	#BJTHF,R1
2$:	LDF	@(R2)+,AC0
	STF	AC0,(R1)+
	SOB	R3,2$

;	LDF	SGJTRN(R5),AC1	;NEW ANGLE VALUE STATMENT NOW CHANGED
	MOV	SGJTRN(R5),R0
	MOV	R4,-(SP)
	MOV	10(SP),R4
	JSR	PC,@LGETARG
	MOV	(SP)+,R4
	MOV	2(R0),R0	; get address of scalar
	LDF	(R0),AC1	;new angle value
	LDF	@THETFP(R4),AC0	;OLD ANGLE VALUE
	CMP	#1,ABSDRV	;ABSOLUTE OR RELATIVE MOVE?
	BEQ	13$
	ADDF	AC0,AC1
13$:	STF	AC1,@THETFP(R4)	;NOW WE HAVE STORED NEW JOINT VALUE IN RELEVANT SPOT
	MOV	#DDARRAY,R0	;LOAD UP RELEVANT DARRAY COEF
	MOV	#DARRAY,R1
	MOV	#12.,R3
10$:	MOV	R0,(R1)+
	ADD	#4,R0
	SOB	R3,10$		;DARRAY NOW CONTAINS DUMMY ARRAY

	MOV	(SP)+,R1		;GET BACK POINTER TO POLYNOMIAL POSITION
	MOV	#SGJOFF,(R1)	;OUTPUT NEXT SEGMENT POINTER
	MOV	#0,SEGTRN(R1)	;OUTPUT ZERO FOR TRANS NUM
	MOV	#0,RNCODE(R1)	;OUTPUT ZERO FOR CODES NUM
	MOV	SGJTME(R5),SEGTME(R1)	;
	MOV	#3000.,SEGTME(R1)	; give it arbitrary 3 second motion
	ADD	#A0COEFF,R1	;GET POSITION OF POLYNOOMIAL
	STF	AC0,(R1)+	;OUTPUT A0
	CLRF	(R1)+		;A1=0
	CLRF	(R1)+		;A2=0
	SUBF	AC1,AC0		;AC0=OLD-NEW
	MULF	FM10,AC0
	STF	AC0,(R1)+	;A3
	MULF	FM1P5,AC0	;
	STF	AC0,(R1)+	;A4
	MULF	FM0P4,AC0	;
	STF	AC0,(R1)+	;A5

	CMP	#10.,R4		;DETERMINE DEVICE
	BLT	3$
	MOV	#YELARM,R2	;IS YELLOW ARM
	BR	8$
3$:	CMP	#12.,R4
	BLT	4$
	MOV	#YELHND,R2	;IS YELLOW HAND
	MOV	R1,DARRAY
	ADD	#4,R1
	MOV	R1,DARRAY+2
	BR	7$
4$:	CMP	#24.,R4
	BLT	5$
	MOV	#BLUARM,R2	;IS BLUE ARM
	SUB	#16,R4		;CORRECT FOR OFFSET
	BR	8$
5$:	MOV	#BLUHND,R2	;IS BLUE HAND
	MOV	R1,DARRAY
	ADD	#4,R1
	MOV	R1,DARRAY+2
	BR	7$
8$:	MOV	R1,DARRAY(R4)
	ADD	#4,R1
	MOV	R1,DARRAY+14(R4)
7$:	CLR	(R1)+
	CLR	(R1)+
	MOV	#DARRAY,R1
	MOV	#BTHF,R0
	JSR	PC,DTERMS	;MAKE A CALL TO BEJCZY

	MOV	#PLYBUF,R0	;SETUP TO CALL MOVE
	MOV	(SP)+,R1	;GET THE DEVICE BLOCK ADRESS
	JSR	PC,MOVE		;WAVE THE ARM
	MOV	(SP)+,R5	;RESTORE REGISTERS
	MOV	(SP)+,R4	;R0,R1 WILL BE IN THE STATE IN WHICH THEY LEFT MOVE
	MOV	(SP)+,R3
	MOV	(SP)+,R2
	RTS	PC		;EXIT
;FOLLOWING CONTAIN JOINT ANGLES FOR BLUE AND YELLOW ARMS AT THE NEW TRANSES
DATA
YBJTHF:
YJTHF:	.BLKW	16
BJTHF:	.BLKW	16
YJTHO:	.BLKW	16
BJTHO:	.BLKW	16
THETFP:
YTHF:	YJTHF
	YJTHF+4
	YJTHF+10
	YJTHF+14
	YJTHF+20
	YJTHF+24
	YJTHF+30
BTHF:	BJTHF
	BJTHF+4
	BJTHF+10
	BJTHF+14
	BJTHF+20
	BJTHF+24
	BJTHF+30

FM10:	.FLT2	-10.0
FM1P5:	.FLT2	-1.5
FM0P4:	.FLT2	-0.4
DARRAY:	.BLKW	14		;PUT POINTERS TO BEJCZY TERMS
DDARRAY: .BLKW	30		;ROOM FOR DUMMY BEJCZY COEFFS THAT ARE NOT NEEDED
;	 0.75,0.5,4.5,0.2,0.2,0.4,5.0,		! Yellow joint times  FROM ARMSOL
;	 0.75,0.75,4.5,0.2,0.2,0.4,5.0;		! Blue joint times IN JIF/DEG
; SO THE FOLLOWING ARE 1000/60 OF THESE VALUES TO GIVE MSEC/DEG
TIMFAC:
YTIMFAC: .FLT2	12.5
	 .FLT2	8.333
	 .FLT2	75.0
	 .FLT2	3.333
	 .FLT2	3.333
	 .FLT2	6.667
	 .FLT2	83.333
BTIMFAC: .FLT2	12.5
	 .FLT2	12.5
	 .FLT2	75.0
	 .FLT2	3.333
	 .FLT2	3.333
	 .FLT2	6.667
	 .FLT2	83.333
SPEED:	 .FLT2	1.0			;DEFAULT SPEED FACTOR
ABSDRV:	1
PLYBUF:	.BLKW	1000
CODE
;ROUTINE TO READ RAW FORCE WRIST AND RETURN INFORMATION TO PDP10
; copied over from old POINTY file
; note that the data sent is in integer format
;
; procedure should be called with R1 containing a pointer to
; the buffer in which the data is to be stored.
; sample call is as follows:
;
;	MOV	#FPPTR,R1
;	JSR	PC,RFORCE
;
;	R0,R2 will be garbaged.
;	FPPTR will be updated
;
RFORCE:	MOV	R3,-(SP)	;save registers
	MOV	R4,-(SP)
	MOV	R1,-(SP)
	MOV	(R1),R1		;now R1 has address instead of address of address

	MOV	#10.,R2		;READ 10 SETS OF DATA

SETLP:	MOV	#9.,R3		;EIGHT STRAIN GAGES IN ALL+REF CHAN
	MOV	#30,R4		;FIRST STRAIN GAGE CHANNEL
REDLP:	MOVB	R4,ADCCHN	;START CONVERTING STRAIN GAGE READING
WLP:    TSTB	ADCSR		;WAIT TILL CONVERSION COMPLETED
	BMI	CNVDNE  
	BR 	WLP
CNVDNE:	MOV	ADCVAL,R0	;GET READING FROM BLUE INTERFACE
;	ADD	#2048.,R0
	MOV	R0,(R1)+	;SAVE READING
	INC	R4		;POINT TO NEXT CHANNEL
	SOB	R3,REDLP	;REPEAT UNTIL DONE
	CLR	-2(R1)		;NO REFERENCE READING
	SOB	R2,SETLP	;DO IT 10. TIMES

	MOV	(SP)+,R0	;now update the pointer value
	MOV	R1,(R0)
	MOV	(SP)+,R4	;retrieve registers
	MOV	(SP)+,R3
	RTS	PC